/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "ckatanasensing.h"

#include <kniBase.h>
#include <cstdlib>


namespace Katana {

void CKatanaSensing::Init() {
	alpha = 100; therehold = 70;
	CSctBase* sensctrl	= &base->GetSCT()->arr[0];		//get pointer to controller
	data = sensctrl->GetDAT();							//get pointer to data
	for(int i = 0;  i < S_NSENSORS; i++) {
		min_value[i] = 255;
		max_value[i] = 0;
	}
}

void CKatanaSensing::UpdateSensors() {
	base->GetSCT()->arr[0].recvDAT();					//update sensor data
	for (int k = 0; k < data->cnt; k++) {
		fsensors[k] = ((100 - alpha)*fsensors[k] + alpha*data->arr[k])/100;
		is_obstacle[k] = std::abs(fsensors[k] - no_obstacle_value[k]) > therehold;
		if (min_value[k] > data->arr[k]) min_value[k] = data->arr[k];
		if (max_value[k] < data->arr[k]) max_value[k] = data->arr[k];
	}
}
	
void CKatanaSensing::CalculateNoObstacleValues(int nsamples) {
	KNI::Timer poll(10);
	int buffer[S_NSENSORS];
	
	for(int i = 0; i < S_NSENSORS; i++) buffer[i] = 0;
	for(int i = 0; i < nsamples; i++) {
		base->GetSCT()->arr[0].recvDAT();
		for (int k = 0; k < data->cnt; k++) buffer[k] += data->arr[k];
		poll.Start();
		poll.WaitUntilElapsed();
	}
	for(int i = 0; i < S_NSENSORS; i++) no_obstacle_value[i] = buffer[i]/nsamples;;
}

bool CKatanaSensing::MotOnTarget(int indx, int mode, int tolerance) {
	const long POLLFREQUENCY = 200;
	bool ret = true;
	CMotBase * mot = &base->GetMOT()->arr[indx];
	
	mot->recvPVP();
	
	if (mot->GetPVP()->msf == MSF_MOTCRASHED) throw MotorCrashException();
	switch(mode) {
		case 0:
			ret = (std::abs(mot->GetTPS()->tarpos - mot->GetPVP()->pos) < tolerance);
			break;
		case 1:
			ret = mot->GetPVP()->msf == MSF_NLINMOV;
			break;
		case 2:
			ret = mot->GetPVP()->msf != MSF_LINMOV;
			break;
	}
	
	return ret;	
}

bool CKatanaSensing::RobotOnTarget( int tolerance ) {
	const TKatMOT* mot = base->GetMOT();
	base->recvMPS(); // get position for all motors
	base->recvGMS(); // get status flags for all motors
	
	for (int idx=0; idx < getNumberOfMotors(); idx++) {
		if (mot->arr[idx].GetPVP()->msf == 40) throw MotorCrashException();
		if  (std::abs(mot->arr[idx].GetTPS()->tarpos - mot->arr[idx].GetPVP()->pos) >= 100) return false;
	}
	return true;
}

}

